package com.hitqz.robot.dispatch.netty.service.impl;

import cn.hutool.core.collection.CollectionUtil;
import cn.hutool.extra.spring.SpringUtil;
import cn.hutool.json.JSONUtil;
import com.alibaba.csp.sentinel.util.StringUtil;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.google.common.cache.Cache;
import com.google.common.cache.CacheBuilder;
import com.google.common.collect.Maps;
import com.hitqz.robot.api.business.dto.CmdRobot2PosDto;
import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
import com.hitqz.robot.api.business.entity.JobModeEntity;
import com.hitqz.robot.api.business.entity.PathResourceEntity;
import com.hitqz.robot.api.business.entity.RobotEntity;
import com.hitqz.robot.api.business.entity.RobotMapPosEntity;
import com.hitqz.robot.api.business.enums.NavEnum;
import com.hitqz.robot.api.business.enums.WorkStatusEnum;
import com.hitqz.robot.api.common.dto.info.ReleaseResourceDto;
import com.hitqz.robot.api.common.dto.info.RobotNavInfoDto;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.dispatch.protocol.dto.FormConfigModalDto;
import com.hitqz.robot.api.dispatch.protocol.dto.FormFieldConfigModalDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyGotoTargetDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
import com.hitqz.robot.biz.service.JobModeService;
import com.hitqz.robot.biz.service.PathResourceService;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.biz.service.RobotService;
import com.hitqz.robot.dispatch.netty.factory.AlgorithmFactory;
import com.hitqz.robot.dispatch.netty.service.DispatchService;
import com.hitqz.robot.dispatch.netty.service.handle.RuleHandleFactory;
import lombok.RequiredArgsConstructor;
import lombok.SneakyThrows;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.Coordinate;
import org.locationtech.jts.geom.GeometryFactory;
import org.locationtech.jts.geom.Point;
import org.springframework.beans.factory.SmartInitializingSingleton;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Service;
import org.springframework.util.StringUtils;
import org.springframework.web.client.RestTemplate;

import java.util.*;
import java.util.concurrent.*;
import java.util.concurrent.locks.ReentrantLock;

/**
 * @author xupkun
 * @date 2024/7/28
 */
@Service
@RequiredArgsConstructor
@Slf4j
public class DispatchServiceImpl implements DispatchService, SmartInitializingSingleton {

    private final AlgorithmFactory algorithmFactory;

    private final PathResourceService pathResourceService;
    private final RuleHandleFactory ruleHandleFactory;
    private final RobotMapPosService robotMapPosService;
    private final RobotService robotService;
    private final JobModeService jobModeService;

    private final RestTemplate restTemplate;

    @Value("${robot.system-type}")
    private String systemType;

    @Value("${robot.client-address1}")
    private String clientAddress1;

    @Value("${robot.client-address2}")
    private String clientAddress2;

    /**
     * key-groupid
     * value - robotId
     */
    private final Map<Integer, String> lastExecutedRobotMap = Maps.newHashMap();


    @Override
    public String findExecuteRobot(Integer groupId, String taskContent) {
        // 使用随机机器人
        String executeRobot = null;
        // 初始化 Map 以避免 NPE
        Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
        Cache<Integer, List<RobotEntity>> robotDispatch = SpringUtil.getBean("robotDispatch");
        // 查询当前组下的所有机器人
        List<RobotEntity> currentGroupRobot = robotDispatch.getIfPresent(groupId);
        // 检查列表是否为空
        if (currentGroupRobot == null) {
            //log.error("not found groupId : " + groupId);
            return null;
        }
        // 过滤出可用的机器人
        //log.info("过滤前可用的机器人数量；{}", currentGroupRobot.size());
        JobModeEntity jobMode = jobModeService.getDefaultJobMode();
        List<String> robotSns = currentGroupRobot.stream()
                .filter(t -> {
                    RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(t.getRobotSn());
                    if (robotClientSummaryDto == null) {
                        return false;
                    }
                    Double battery = Double.parseDouble(robotClientSummaryDto.getStatus().getBattery());
                    if (battery <= 0.0) {
                        return false;
                    }
                    if (robotClientSummaryDto.getLogicOnlineStatus() == 0) {
                        return false;
                    }
                    int workStatus = robotClientSummaryDto.getStatus().getWorkStatus();
                    Boolean isFull = robotClientSummaryDto.getStatus().getIsFull();
                    if ("lundui".equals(systemType)) {//轮对
                        if (isFull != null && isFull) {
                            //双车模式下 带载不执行
                            if ("2".equals(jobMode.getMode())) {
                                return false;
                            }
                            //单车模式下 带载可以执行
                            if ("1".equals(jobMode.getMode()) && workStatus == Integer.parseInt(WorkStatusEnum.FREE.getCode())) {
                                log.info("轮对，单车模式下，空闲并且带载的车可以执行任务：{}", t.getRobotSn());
                                return true;
                            }
                        }
                    }
                    //轮轴 带载的车不让执行
                    if ("lunzhou".equals(systemType) && isFull != null && isFull) {
                        return false;
                    }
                    return robotClientSummaryDto.getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.FREE.getCode());
                })
                .map(RobotEntity::getRobotSn)
                .toList();

        Cache<String, RobotNavInfoDto> robotNavInfoDtoMapCache = SpringUtil.getBean("robotNavInfo");

        //轮对单车模式下，就算可用AGV两个，同一时间内也只允许一辆AGV执行任务
        if ("lundui".equals(systemType) && "1".equals(jobMode.getMode())) {
            for (RobotNavInfoDto robotNavInfoDto : robotNavInfoDtoMapCache.asMap().values()) {
                if (WorkStatusEnum.WORK.getCode().equals(robotNavInfoDto.getWorkStatus())) {
//                    log.info("轮对，单车模式下，已有AGV {}正在执行任务，不能再执行任务", robotNavInfoDto.getRobotSn());
                    return null;
                }
            }
        }

        //轮对双车模式下 ，且有两个可用机器人 需要根据机器人当前位置再筛选
        if ("lundui".equals(systemType) && "2".equals(jobMode.getMode())) {
            List<String> newRobotSns = new ArrayList<>();
            //如果机器人不在 CP14、CP15、LM11 、 LM9、 LM7、 LM8 则不允许接任务
            List<String> allowList = Arrays.asList("CP14", "CP15", "LM11", "LM9", "LM7", "LM8");
            for (String robotSn : robotSns) {
                RobotNavInfoDto robotNavInfoDto = robotNavInfoDtoMapCache.getIfPresent(robotSn);
                String currentPosCode = robotNavInfoDto.getCurrentPos().getCode();
                if (allowList.contains(currentPosCode)) {
                    newRobotSns.add(robotSn);
                }
            }
            robotSns = newRobotSns;
        }

//        log.info("可用的机器人数量:" + robotSns.size());
        if (CollectionUtil.isEmpty(robotSns)) {
            return null;
        }

        //如果可用机器人数量大于1个，则选择距离目标最近的机器人去执行
        if (robotSns.size() > 1) {
            try {
                //单车模式下，继续选择上次执行的机器人去执行
                if ("lundui".equals(systemType) && "1".equals(jobMode.getMode()) && lastExecutedRobotMap.size() > 0) {
                    for (String robotSn : robotSns) {
                        if (robotSn.equals(lastExecutedRobotMap.get(groupId))) {
                            executeRobot = robotSn;
                            log.info("单车模式下，继续选择上次执行的机器人{} 去执行==>", executeRobot);
                            break;
                        }
                    }
                }

                if (executeRobot == null) {
                    log.info("选择距离目标最近的机器人去执行==>");
                    executeRobot = findNearestRobot(robotSns, taskContent);
                }

            } catch (Exception e) {
                log.error(e.getMessage(), e);
            }
        } else {
            executeRobot = robotSns.get(0);

            String lastExecutedRobot = lastExecutedRobotMap.get(groupId);
            //上一次执行的机器人在线，且空闲，且当前是单车模式，则应该还由上个机器人继续执行
            if ("lundui".equals(systemType) && "1".equals(jobMode.getMode())
                    && StringUtils.hasText(lastExecutedRobot)
                    && !lastExecutedRobot.equals(executeRobot)) {
                RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(lastExecutedRobot);
                if (robotClientSummaryDto.getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.FREE.getCode())
                        && robotClientSummaryDto.getLogicOnlineStatus() == 1) {
                    executeRobot = lastExecutedRobot;
                    log.info("单车模式下，上次执行的机器人{} 仍然在线且空闲，应该还由上次机器人继续执行==>", executeRobot);
                }
            }
        }
        //  更新最后执行的机器人
        if (executeRobot != null) {
            if ("lundui".equals(systemType) && "1".equals(jobMode.getMode())) {
                lastExecutedRobotMap.put(groupId, executeRobot);
            }
        }
        log.info("最后查询到执行任务的机器人: {}", executeRobot);
        return executeRobot;
    }

    private String findNearestRobot(List<String> robotSns, String taskContent) throws Exception {
        FormConfigModalDto dto = JSONObject.parseObject(taskContent, FormConfigModalDto.class);
        Map<String, Object> taskFields = Maps.newHashMap();
        for (FormFieldConfigModalDto field : dto.getFields()) {
            String currentValue = field.getCurrentValue();
            String name = field.getName();
            taskFields.put(name, currentValue);
        }
        //获取目标点位code
        String sourcePosName = Optional.ofNullable(taskFields.get("sourcePosName"))
                .orElse(taskFields.get("sourcePosCode")).toString();
        RobotEntity robotEntity = robotService.selectByRobotSn(robotSns.get(0));
        RobotMapPosEntity sourceMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
                .eq(RobotMapPosEntity::getName, sourcePosName)
                .eq(RobotMapPosEntity::getMapCode, robotEntity.getMapCode()));
        if (sourceMapPos == null) {
            sourceMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
                    .eq(RobotMapPosEntity::getCode, sourcePosName)
                    .eq(RobotMapPosEntity::getMapCode, robotEntity.getMapCode()));
        }
        if ((sourcePosName.equals("91101")
                || sourcePosName.equals("91102") || sourcePosName.equals("911005") || sourcePosName.equals("911006"))
                && sourceMapPos == null) {
            String srcnum = Optional.ofNullable(taskFields.get("srcnum")).orElse("").toString();
            String reName = sourcePosName + "-" + srcnum;
            sourceMapPos = robotMapPosService.getOne(Wrappers.<RobotMapPosEntity>lambdaQuery()
                    .eq(RobotMapPosEntity::getName, reName)
                    .eq(RobotMapPosEntity::getMapCode, robotEntity.getMapCode()));
        }
        final String sourcePosCode = sourceMapPos.getCode();
        //查询路径 生成总距离
        Map<String, Double> robotDistanceMap = Maps.newHashMap();
        robotSns.stream().parallel().forEach(robotSn -> {
            Cache<String, RobotNavInfoDto> robotNavInfoDtoMapCache = SpringUtil.getBean("robotNavInfo");
            RobotNavInfoDto robotNavInfoDto = robotNavInfoDtoMapCache.getIfPresent(robotSn);
            String currentPosCode = robotNavInfoDto.getCurrentPos().getCode();
            List<RobotMapPos> list = null;
            while (list == null) {
                list = getNavPath(clientAddress1, currentPosCode, sourcePosCode);
            }
            Double totalDistance = calculateDistance(list);
            robotDistanceMap.put(robotSn, totalDistance);
            log.info("机器人:{} 当前点位：{},目标点位：{},点位距离集合：{},总距离：{}",
                    robotSn, currentPosCode, sourcePosCode, list.stream().map(RobotMapPos::getCode).toList(), totalDistance);
        });


        // 找出值最小的键值对
        Map.Entry<String, Double> minEntry = robotDistanceMap.entrySet()
                .stream()
                .min(Map.Entry.comparingByValue())
                .orElseThrow(() -> new RuntimeException("Map is empty"));
        //最近机器人
        String nearestRobot = minEntry.getKey();
        return nearestRobot;
    }

    private Double calculateDistance(List<RobotMapPos> list) {
        GeometryFactory geometryFactory = new GeometryFactory();
        Double distance = 0.0;
        for (int i = 0; i < list.size() - 1; i++) {
            RobotMapPos pos1 = list.get(i);
            RobotMapPos pos2 = list.get(i + 1);
            Point point1 = geometryFactory.createPoint(new Coordinate(pos1.getPosX(), pos1.getPosY()));
            Point point2 = geometryFactory.createPoint(new Coordinate(pos2.getPosX(), pos2.getPosY()));
            double distance1 = point1.distance(point2);
            distance += distance1;
        }
        return distance;
    }


    /**
     * 查询导航路径
     *
     * @param clientAddress
     * @param sourceCode
     * @param targetCode
     * @return
     */
    protected List<RobotMapPos> getNavPath(String clientAddress, String sourceCode, String targetCode) {
        try {
            String url = clientAddress + "/ctrRobot/getNavPathWithCode?sourceCode={param1}&targetCode={param2}";
            Map<String, String> params = new HashMap<>();
            params.put("param1", sourceCode);
            params.put("param2", targetCode);
            String response = restTemplate.getForObject(url, String.class, params);
            List<RobotMapPos> list = JSONUtil.toList(JSONUtil.parseArray(response), RobotMapPos.class);
            List<String> posList = list.stream().map(RobotMapPos::getCode).toList();
            while (!sourceCode.equals(posList.get(0)) || !targetCode.equals(posList.get(posList.size() - 1))) {
                log.warn("getNavPath返回结果错误！sourceCode：{} targetCode：{} 返回路径：{}", sourceCode, targetCode, posList);
                TimeUnit.SECONDS.sleep(1);
                response = restTemplate.getForObject(url, String.class, params);
                list = JSONUtil.toList(JSONUtil.parseArray(response), RobotMapPos.class);
                posList = list.stream().map(RobotMapPos::getCode).toList();
                log.warn("重新获取结果:{}", posList);
            }
            return list;
        } catch (Exception e) {
            log.error("查询导航路径失败！sourceCode: {}, targetCode: {}  ,error: {}", sourceCode, targetCode, e.getMessage());
            try {
                TimeUnit.SECONDS.sleep(1);
            } catch (InterruptedException ex) {
            }
            log.warn("重新获取请求getNavPath!");
            return getNavPath(clientAddress, sourceCode, targetCode);
        }
    }

    @Override
    public void calAlgorithm(String algorithm, Integer groupId, List<RobotEntity> robotEntityList) {
        algorithmFactory.getRoutingStrategy(algorithm).calculate(groupId, robotEntityList);
    }

    //轮对、轮轴机器人待命点位
    Map<String, List<String>> standbyPos = Map.of("lundui", Arrays.asList("LM7", "LM8"),
            "lunzhou", Arrays.asList("LM1", "LM34"));


    private final static ReentrantLock handleLock = new ReentrantLock();
    private final static ReentrantLock applyGotoTargetLock = new ReentrantLock();
    private final static ReentrantLock lockReleasePathLock = new ReentrantLock();

    @Override
    public void handle(RobotNavInfoDto dto) {
        try {
            handleLock.lock();
            //实时更新机器人导航数据
            Cache<String, RobotNavInfoDto> robotNavInfo = SpringUtil.getBean("robotNavInfo");
            robotNavInfo.put(dto.getRobotSn(), dto);
        } finally {
            handleLock.unlock();
        }
    }

    @Override
    public void releaseResource(ReleaseResourceDto dto) {
        pathResourceService.update(Wrappers.<PathResourceEntity>lambdaUpdate()
                .set(PathResourceEntity::getRobotSn, "")
                .set(PathResourceEntity::getStatus, "0")
                .eq(PathResourceEntity::getRobotSn, dto.getRobotSn()));
    }

    @Override
    @SneakyThrows
    public void releaseResourceExcludeSelf(ReleaseResourceDto dto) {
        Cache<String, RobotNavInfoDto> robotNavInfoDtoMapCache = SpringUtil.getBean("robotNavInfo");
        RobotNavInfoDto current = robotNavInfoDtoMapCache.getIfPresent(dto.getRobotSn());
        String currentPos = current.getCurrentPos() == null ? "" : current.getCurrentPos().getCode();
        while (!StringUtils.hasText(currentPos)) {
            robotNavInfoDtoMapCache = SpringUtil.getBean("robotNavInfo");
            current = robotNavInfoDtoMapCache.getIfPresent(dto.getRobotSn());
            currentPos = current.getCurrentPos() == null ? "" : current.getCurrentPos().getCode();
            TimeUnit.SECONDS.sleep(1);
        }
        final String currentPosCode = currentPos;
        List<PathResourceEntity> allList = pathResourceService.list();
        List<PathResourceEntity> resetList = allList.stream()
                .filter(resource -> {
                    String robotSn = resource.getRobotSn();
                    List<String> posList = List.of(resource.getPathList().split(","));
                    if (StringUtils.hasText(robotSn) && robotSn.equals(dto.getRobotSn()) && !posList.contains(currentPosCode)) {
                        return true;
                    }
                    return false;
                }).toList();
        List<Long> resetIds = resetList.stream().map(PathResourceEntity::getId).toList();
        if (!resetIds.isEmpty()) {
            pathResourceService.update(Wrappers.<PathResourceEntity>lambdaUpdate()
                    .set(PathResourceEntity::getRobotSn, "")
                    .set(PathResourceEntity::getStatus, "0")
                    .in(PathResourceEntity::getId, resetIds));
        }
    }

    @Override
    public List<RobotNavInfoDto> getRobotNavInfoDtoList() {
        Cache<String, RobotNavInfoDto> robotNavInfoDtoMapCache = SpringUtil.getBean("robotNavInfo");
        return robotNavInfoDtoMapCache.asMap()
                .values().stream().toList();
    }

    @Override
    public void applyGotoTarget(RobotApplyGotoTargetDto dto) {
        try {
            applyGotoTargetLock.lock();
            log.info("机器人：{} 当前位置：{} 请求前往目标点：{}", dto.getClientId(), dto.getCurrentPos(), dto.getTargetPos());
            //取消上一个调度前往目标
            ruleHandleFactory.getRuleHandle(systemType).cancelGotoTarget(dto.getClientId());
            //下发当前调度前往目标
            ruleHandleFactory.getRuleHandle(systemType).applyGotoTarget(dto);
        } catch (Exception e) {
            log.error(e.getMessage(), e);
        } finally {
            applyGotoTargetLock.unlock();
        }
    }

    /**
     * 取消调度前往目标
     *
     * @param dto
     */
    @Override
    public void cancelGotoTarget(ReleaseResourceDto dto) {
        ruleHandleFactory.getRuleHandle(systemType).cancelGotoTarget(dto.getRobotSn());
    }

    /**
     * 释放 路径资源
     */
    @Scheduled(fixedDelay = 200L)
    public void releasePathResource() {
        try {
            lockReleasePathLock.lock();
            List<PathResourceEntity> pathResourceEntityList = pathResourceService.list();
            for (PathResourceEntity pathResourceEntity : pathResourceEntityList) {
                if ("2".equals(pathResourceEntity.getStatus())) {//已经过占用的资源 ，需要释放掉
                    pathResourceEntity.setRobotSn("");
                    pathResourceEntity.setStatus("0");
                    pathResourceService.updateById(pathResourceEntity);
                }
            }
            Cache<String, RobotNavInfoDto> robotNavInfoDtoMapCache = SpringUtil.getBean("robotNavInfo");
            for (RobotNavInfoDto dto : robotNavInfoDtoMapCache.asMap().values()) {
                String currentPos = dto.getCurrentPos().getCode();
                //机器人非作业中 且 回到待命点
                if (!WorkStatusEnum.WORK.getCode().equals(dto.getWorkStatus())
                        && standbyPos.get(systemType).contains(currentPos)) {
                    pathResourceService.update(Wrappers.<PathResourceEntity>lambdaUpdate()
                            .set(PathResourceEntity::getRobotSn, "")
                            .set(PathResourceEntity::getStatus, "0")
                            .eq(PathResourceEntity::getRobotSn, dto.getRobotSn())
                    );
                }
            }
        } catch (Exception e) {
            log.error("释放 路径资源异常{}", e.getMessage());
        } finally {
            lockReleasePathLock.unlock();
        }
    }


    @Override
    public void afterSingletonsInstantiated() {
        //初始化数据
        pathResourceService.update(Wrappers.<PathResourceEntity>lambdaUpdate()
                .set(PathResourceEntity::getRobotSn, "")
                .set(PathResourceEntity::getStatus, "0"));

    }
}
